function [gbest,gbestval,fitcount,fitness,gbestval_history]= FCO_func(fhd,Dimension,PopulationSize,MaxIter,lowerbound,upperbound,pos,bool_Plot,varargin)

rand('state',sum(100*clock));
mi = MaxIter;
ps = PopulationSize;
D = Dimension;
[pub, plb, vub, vlb] = SetBound(D,ps,lowerbound,upperbound);

%  Parameters of the Algorithm //////////////////////////////////////
cc=[2 2];  % For PSO
iwt=0.9-(1:mi).*(0.5./mi); % For PSO
G0 = 1.5; % For SGSA
epsilon = 1e-3; % For SGSA
g = 0.13; % For QGSA
NumSC = 4; % For Social Class
%  Parameters of the Algorithm //////////////////////////////////////

%  Settings of the Algorithm //////////////////////////////////////
% If ps = 50, NumSC = 4
% size_each_level = 12    12    12    14
% lb_level = 1    13    25    37
% ub_level = 12    24    36    50
size_each_level = repmat(floor(ps/NumSC),1,NumSC);
size_each_level(NumSC) = ps - (size_each_level(1)*(NumSC-1));
lb_level(1,1) = 1;
ub_level(1,1) = size_each_level(1);
for l = 2 : NumSC
    lb_level(l,1) = sum(size_each_level(1,1:l-1))+1;
    ub_level(l,1) = sum(size_each_level(1,1:l));
end
%  Settings of the Algorithm //////////////////////////////////////

vel = zeros(ps,D); % initialize the velocity of the particles
acc = zeros(ps,D); % initialize the acceleration of the particles

fitness = feval(fhd,pos', varargin{:});
fitcount = ps;
pbest = pos;
pbestval = fitness;
[gbest_fit, gbest_id] = min(pbestval);
gbest = pbest(gbest_id, :);
gbestrep = repmat(gbest, ps, 1);
cbest_fit = min(fitness); % Current best
cworst_fit = max(fitness); % Current worst
gbestval = gbest_fit;
gbestval_history = zeros(fix(mi/500),2);

if(bool_Plot)
    h = figure(8);
    haxes = plot( 0 , 0 );
    XArray = [];
    YArray = [];
    title('FCO');
end
%     h = figure(8);
%     haxes = plot( 0 , 0 );
%     XArray = [];
%     YArray = [];
%     title('FCO');
dcc1 = ones(ps,D).*2;
dcc2 = ones(ps,D).*2;
diwt = ones(ps,D).*0.5;
leader = zeros(NumSC,D);
leader_id = zeros(NumSC,1);
leader_fit = zeros(NumSC,1);
leader_rep = zeros(ps,D);
temp_pos = zeros(ps,D);
temp_mass = zeros(ps,1);

if(cbest_fit == cworst_fit)
    mass = repmat((1/ps),ps,1);
else
    mass = (fitness'- cworst_fit) / (cbest_fit-cworst_fit);
    mass = mass / sum(mass);
end
sort_mass = [mass (1:ps)'];
sort_mass = sortrows(sort_mass,1);
std_mass = std(mass);

% Decide leaders for each country
dis = zeros(ps,1);
for n = 1: ps
    dis(n,1) = norm(pos(n,:)-gbest);
end
sort_dis = [dis (1:ps)'];
sort_dis = sortrows(sort_dis,1);
for l = 1 : NumSC
%     leader_id(l,1) = sort_mass(ps-l+1,2);
    leader_id(l,1) = sort_dis(lb_level(l,1),2);
    leader(l,:) = pos(leader_id(l,1),:);  
    leader_fit(l,1) = fitness(leader_id(l,1));  
    temp_pos(ub_level(l,1),:) = leader(l,:);
    temp_mass(ub_level(l,1),:) = mass(leader_id(l,1),:);
end
count = 1;
for n = 1 : ps        
    if(sum(n==ub_level) == 0)            
        while(sum(count==leader_id))
            count = count + 1;
        end
        temp_pos(n,:) = pos(count,:);
        temp_mass(n,:) = mass(count,:);
        count = count + 1;
    end
end
pos = temp_pos;
mass = temp_mass;
leader_fit_histroy = zeros(NumSC,mi);
leader_fit_histroy(:,1) = leader_fit;
for iter = 2 : mi  
    for l = 1 : NumSC
        leader_fit_histroy(l,iter) = leader_fit(l,1);
        if(mod(iter,100) == 0) % Choose new leader
            [leader_fit(l,1), leader_id(l,1)] = min(fitness(lb_level(l):ub_level(l)));
            leader(l,:) = pos(leader_id(l,1), :);
        end
    end

    for l = 1 : NumSC % Announce leader for each country
        leader_rep(lb_level(l):ub_level(l),:) = repmat(leader(l,:),size_each_level(l),1);
    end
    
%     acc = cc(1).*rand(ps,D).*(pbest-pos) + cc(2).*rand(ps,D).*(gbestrep-pos);
    if(mod(iter,1000) <= 500)
        acc = cc(1).*rand(ps,D).*(pbest-pos) + cc(2).*rand(ps,D).*(leader_rep-pos);
    else
        acc = cc(1).*rand(ps,D).*(pbest-pos) + cc(2).*rand(ps,D).*(gbestrep-pos);
    end
    
    % Leader movement
    if(iter ~= 2)
        leader_best = min(leader_fit);
        leader_worst = max(leader_fit);
        if(leader_best == leader_worst)
            leader_mass = repmat((1/NumSC),NumSC,1);
        else
            leader_mass = (leader_fit-leader_worst) / (leader_best-leader_worst);
            leader_mass = leader_mass / sum(leader_mass);
        end
        leader_mass_sort = [leader_mass (1:NumSC)'];
        leader_mass_sort = sortrows(leader_mass_sort, 1);
        for l = 1 : NumSC % Global best, i.e. the best leader
            if(l == leader_mass_sort(NumSC,2))
%                 id = leader_id(l);
%                 acc(id,:) = cc(1).*rand(1,D).*(pbest(id,:)-pos(id,:)) + ...
%                             cc(2).*rand(1,D).*(gbest-pos(id,:));
            else
                force = zeros(1,D);
                for k = 1 : NumSC
                    d = leader(l,:) - leader(k,:); % Repulsive force
                    force = force + (leader_mass(k)>leader_mass(l)) * d.* rand(1,D) * leader_mass(k) / (norm(d) + epsilon);
%                     force = force + d.* rand(1,D) * leader_mass(k) / (norm(d) + epsilon);
                end
                acc(leader_id(l),:) = G0 * force;
            end
        end
    end
%     vel = iwt(iter).*vel + acc;
    vel = 0.5.*vel + acc;
%     vel = rand(ps,D).*vel + acc;
        
    vel = ((vel>=vlb)&(vel<=vub)).*vel+(vel<vlb).*vlb+(vel>vub).*vub;
    pos = pos+vel;
    pos=((pos>=plb)&(pos<=pub)).*pos...
        +(pos<plb).*(plb+0.25.*(pub-plb).*rand(ps,D))+(pos>pub).*(pub-0.25.*(pub-plb).*rand(ps,D));
    
    fitness = feval(fhd,pos', varargin{:});
    fitcount = fitcount+ps;
    tmp = (pbestval < fitness);
    temp=repmat(tmp',1,D);
    pbest=temp.*pbest+(1-temp).*pos;
    pbestval=tmp.*pbestval+(1-tmp).*fitness; % update the pbest    
    [gbest_fit, gbest_id] = min(pbestval);
    gbest = pbest(gbest_id, :);
    gbestrep = repmat(gbest, ps, 1);
    cbest_fit = min(fitness); % Current best
    cworst_fit = max(fitness); % Current worst
    gbestval = gbest_fit;
        
    if(bool_Plot & mod(iter,10)== 0)
        XArray = [ XArray iter]; 
        YArray = [ YArray gbestval];
        set( haxes , 'XData' , XArray , 'YData' , YArray );
        drawnow
    end
    
    if(mod(iter,500) == 0)
        gbestval_history(fix(iter/500),1) = gbestval;
        gbestval_history(fix(iter/500),2) = iter;
    end
end

h = subplot(4,2,2);
subplot(2,2,1);
plot(leader_fit_histroy(1,:));
dis1 = norm(leader(1,:)-leader(2,:));
dis2 = norm(leader(1,:)-leader(3,:));
dis3 = norm(leader(1,:)-leader(4,:));
dis = [num2str(dis1) ', '  num2str(dis2) ', '  num2str(dis3)];
title(['Leader 1: ' dis]);
subplot(2,2,2);
plot(leader_fit_histroy(2,:));
dis1 = norm(leader(2,:)-leader(1,:));
dis2 = norm(leader(2,:)-leader(3,:));
dis3 = norm(leader(2,:)-leader(4,:));
dis = [num2str(dis1) ', ' num2str(dis2) ', ' num2str(dis3)];
title(['Leader 2: ' dis]);
subplot(2,2,3);
plot(leader_fit_histroy(3,:));
dis1 = norm(leader(3,:)-leader(1,:));
dis2 = norm(leader(3,:)-leader(2,:));
dis3 = norm(leader(3,:)-leader(4,:));
dis = [num2str(dis1) ', ' num2str(dis2) ', ' num2str(dis3)];
title(['Leader 3: ' dis]);
subplot(2,2,4);
plot(leader_fit_histroy(4,:));
dis1 = norm(leader(4,:)-leader(1,:));
dis2 = norm(leader(4,:)-leader(2,:));
dis3 = norm(leader(4,:)-leader(3,:));
dis = [num2str(dis1) ', ' num2str(dis2) ', ' num2str(dis3)];
title(['Leader 4: ' dis]);

end


